#include "motor.h"

#define MOTOR_L_IN1_PIN		GPIO_PIN_1
#define MOTOR_L_IN2_PIN		GPIO_PIN_0
#define MOTOR_L_IN1_PORT 	GPIOA
#define MOTOR_L_IN2_PORT	GPIOA

#define MOTOR_R_IN1_PIN		GPIO_PIN_1
#define MOTOR_R_IN2_PIN		GPIO_PIN_10
#define MOTOR_R_IN1_PORT 	GPIOB
#define MOTOR_R_IN2_PORT	GPIOB

void tim2_init(void)
{	
	__HAL_RCC_AFIO_CLK_ENABLE();
	__HAL_RCC_TIM2_CLK_ENABLE();	
	
	TIM2->PSC = 72-1;
	TIM2->ARR = 1000;
	
	TIM2->CCMR2 |= 6<<12;
	TIM2->CCMR2 |= 1<<11;
	TIM2->CCMR2 |= 6<<4;
	TIM2->CCMR2 |= 1<<3;
	
	TIM2->CCER |= 1<<8;
	TIM2->CCER |= 1<<12;
	
	TIM2->CCR3 = 1000;
	TIM2->CCR4 = 1000;
	
	TIM2->CR1 |= 0x8000;
	TIM2->CR1 |= 0x01;
}

void motor_init(void)
{
	GPIO_InitTypeDef GPIO_Initure;
	
	/* PWM GPIO */
	GPIO_Initure.Pin = GPIO_PIN_2 | GPIO_PIN_3;
	GPIO_Initure.Mode = GPIO_MODE_AF_PP;
	GPIO_Initure.Speed = GPIO_SPEED_FREQ_HIGH;
	HAL_GPIO_Init(GPIOA, &GPIO_Initure);
	
	/* MOTOR L GPIO */
	GPIO_Initure.Pin = MOTOR_L_IN1_PIN | MOTOR_L_IN2_PIN;
	GPIO_Initure.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_Initure.Speed = GPIO_SPEED_FREQ_HIGH;
	HAL_GPIO_Init(MOTOR_L_IN1_PORT, &GPIO_Initure);
	HAL_GPIO_WritePin(MOTOR_L_IN1_PORT,MOTOR_L_IN1_PIN | MOTOR_L_IN2_PIN,GPIO_PIN_SET);
	
	/* MOTOR R GPIO */
	GPIO_Initure.Pin = MOTOR_R_IN1_PIN | MOTOR_R_IN2_PIN;
	GPIO_Initure.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_Initure.Speed = GPIO_SPEED_FREQ_HIGH;
	HAL_GPIO_Init(MOTOR_R_IN1_PORT, &GPIO_Initure);
	HAL_GPIO_WritePin(MOTOR_R_IN1_PORT,MOTOR_R_IN1_PIN | MOTOR_R_IN2_PIN,GPIO_PIN_SET);
	
	tim2_init();
}

void motor_L(uint32_t pwm, int dir)
{
	if(dir==1) {
		HAL_GPIO_WritePin(MOTOR_L_IN1_PORT,MOTOR_L_IN1_PIN,GPIO_PIN_SET);
		HAL_GPIO_WritePin(MOTOR_L_IN2_PORT,MOTOR_L_IN2_PIN,GPIO_PIN_RESET);
	} else {
		HAL_GPIO_WritePin(MOTOR_L_IN1_PORT,MOTOR_L_IN1_PIN,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR_L_IN2_PORT,MOTOR_L_IN2_PIN,GPIO_PIN_SET);
	}
	
	if(pwm>1000) {pwm=1000;}
	TIM2->CCR3 = pwm;
}

void motor_R(uint32_t pwm, int dir)
{
	if(dir==1) {
		HAL_GPIO_WritePin(MOTOR_R_IN1_PORT,MOTOR_R_IN1_PIN,GPIO_PIN_SET);
		HAL_GPIO_WritePin(MOTOR_R_IN2_PORT,MOTOR_R_IN2_PIN,GPIO_PIN_RESET);
	} else {
		HAL_GPIO_WritePin(MOTOR_R_IN1_PORT,MOTOR_R_IN1_PIN,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR_R_IN2_PORT,MOTOR_R_IN2_PIN,GPIO_PIN_SET);
	}
	
	if(pwm>1000) {pwm=1000;}
	TIM2->CCR4 = pwm;
}

